# 船舶状态
import numpy as np
from pvsim import geo
from scipy import constants
from pvsim import pv


class Ship:
    """
    定义船舶的属性与方法, 采用船体坐标系, y 轴指向左舷
    """

    area = np.empty([4, 1])  # 四气室面积
    xc = np.empty([4, 1])  # 四气室形心横坐标
    yc = np.empty([4, 1])  # 四气室形心纵坐标
    vol = np.zeros([4, 1])  # 四气室体积
    p = np.zeros([4, 1])  # 四气室压强
    x = np.zeros([3, 1])  # 船舶运动
    v = np.zeros([3, 1])  # 船舶速度

    an = np.empty([3, 4])  # 几何矩阵
    m = np.zeros([3, 3])  # 质量矩阵

    def __init__(self, cfg):
        """
        初始化船舶的属性
        """
        self.area[0, 0] = geo.get_area(cfg["xsf"] - cfg["xsm"], cfg["ysl"] - 0)
        self.area[1, 0] = geo.get_area(cfg["xsf"] - cfg["xsm"], 0 - cfg["ysr"])
        self.area[2, 0] = geo.get_area(cfg["xsm"] - cfg["xsb"], cfg["ysl"] - 0)
        self.area[3, 0] = geo.get_area(cfg["xsm"] - cfg["xsb"], 0 - cfg["ysr"])
        self.xc[0, 0] = (cfg["xsf"] + cfg["xsm"]) / 2
        self.xc[1, 0] = (cfg["xsf"] + cfg["xsm"]) / 2
        self.xc[2, 0] = (cfg["xsm"] + cfg["xsb"]) / 2
        self.xc[3, 0] = (cfg["xsm"] + cfg["xsb"]) / 2
        self.yc[0, 0] = (cfg["ysl"] + 0) / 2
        self.yc[1, 0] = (cfg["ysr"] + 0) / 2
        self.yc[2, 0] = (cfg["ysl"] + 0) / 2
        self.yc[3, 0] = (cfg["ysr"] + 0) / 2
        self.an[0, :] = self.area[:, 0]
        self.an[1, :] = -self.area[:, 0] * self.xc[:, 0]
        self.an[2, :] = self.area[:, 0] * self.yc[:, 0]
        self.m[0, 0] = cfg["m"]
        self.m[1, 1] = cfg["iy"]
        self.m[2, 2] = cfg["iy"] / 2

    def set_static_pressure(self):
        """
        获取船舶的静态压力
        """
        wm = np.reshape([self.m[0, 0] * constants.g, 0, 0], (3, 1))
        self.p = np.linalg.pinv(self.an) @ wm

    def set_static_volume(self):
        """
        获取船舶的静态体积
        """
        self.vol[0, 0] = pv.p2v_fc(self.p[0, 0])
        self.vol[1, 0] = pv.p2v_fc(self.p[1, 0])
        self.vol[2, 0] = pv.p2v_bc(self.p[2, 0])
        self.vol[3, 0] = pv.p2v_bc(self.p[3, 0])

    def set_posture(self, h):
        """
        获取船舶的姿态
        """
        a = np.empty([4, 3])
        a[:, 0] = 1
        a[:, 1] = -self.xc[:, 0]
        a[:, 2] = self.yc[:, 0]
        self.x = np.linalg.pinv(a) @ (self.vol / self.area + h)

        # print((a @ self.x - h) * self.area - self.vol)
        # exit()
